/*
 * @Author: LVGRAPE
 * @Date: 2023-09-08 17:56:24
 * @LastEditTime: 2025-06-25 17:01:07
 * @LastEditors: LVGRAPE
 * @Description:
 * @FilePath: \zino-fc-v4\ZINO\rc\rc.c
 * 要啥没啥，爱咋咋的
 */

#include "drv_common.h"
#include <rtconfig.h>
#include <rtthread.h>
#include "rc.h"
// #include "fc_error.h"
#include "zino.h"
#include "maths.h"
#include "si24r1_port.h"
#include "si24r1.h"

// #include "fc_error.h"

#define DBG_TAG "2.4G"
#define DBG_LVL DBG_LOG
#include <rtdbg.h>

#define CRC_MAGICBEE 0XED
#define SI24R1_SPI_DEV_NAME "spi_nrf"
#define SI24R1_CE_PIN PA15
#define SI24R1_IRQ_PIN PC11

ZINO_CONFIG_REGISTER(struct rc_pair_data_s, pair_data, 3, 1);

// const static char* ROLE_TABLE[] = { "PTX", "PRX" };
static struct rchandle rchandle =
    {
        // .pair_addr = "UAVI1",
        // .pair_channel = 33,
        // .pair_fc_handshake_byte = 0xfc,
        // .pair_rc_handshake_byte = 0x9c,
        // .pair_handshake_byte = 0x0A,
        // .pair_handshake_byte_v1 = 0x1A,
        // .pair_sync_time = 0,
        // .pair_wait_time = 30,
        // .pairConfirmTime = 0,

};
/**
 * @brief 接收完成处理函数
 *
 * @param nrf24
 * @param data
 * @param len
 * @param pipe
 */
static void rx_ind(struct si24r1 *s, uint8_t *data, uint8_t len, int pipe)
{
    /*! Don't need to care the pipe if the role is ROLE_PTX */
    // rt_kprintf("(p%d)_%d: ", pipe, len);
    // static uint8_t ackCnt = 0;
    // uint8_t crc = crc8_dvb_s2_update(CRC_MAGICBEE, data, len - 1);
    // if (crc == data[len - 1])
    // {
    //     // LOG_D("Crc ok!");
    //     rchandle.rxCnt++;
    //     rt_memcpy(rchandle.rx_msg.raw, data, len);
    //     // rt_kprintf("rchandle.rx_msg.raw:(%d) ", rchandle.mode);
    //     // for (int i = 0;i < 20;i++)
    //     // {
    //     //     rt_kprintf("%02X ", rchandle.rx_msg.raw[i]);
    //     // }
    //     // rt_kprintf("\n");
    //     // LOG_D("RC:h%02x i%02x %d \t %d \t %d \t %d \t %d \t %X \t %X \t %d \t %dHz",
    //     //     rchandle.rx_msg.v.head,
    //     //     rchandle.rx_msg.v.index,
    //     //     (int16_t)rchandle.rx_msg.v.channel[0],
    //     //     (int16_t)rchandle.rx_msg.v.channel[1],
    //     //     (int16_t)rchandle.rx_msg.v.channel[2],
    //     //     (int16_t)rchandle.rx_msg.v.channel[3],
    //     //     (int16_t)rchandle.rx_msg.v.channel[4],
    //     //     (int16_t)rchandle.rx_msg.v.channel[5],
    //     //     (int16_t)rchandle.rx_msg.v.channel[6],
    //     //     (int16_t)rchandle.rx_msg.v.channel[7],
    //     //     rchandle.rx_rssi
    //     // );
    //     if (rchandle.mode == RC_MODE_PAIRING) // return pairing data
    //     {
    //         if (rchandle.rx_msg.v.head == WL_MSG_HEAD_PAIR_REQ) // 回复新地址
    //         {
    //             rchandle.newAddress[0] = rchandle.rx_msg.raw[2];
    //             rchandle.newAddress[1] = rchandle.rx_msg.raw[3];
    //             rchandle.newAddress[2] = rchandle.rx_msg.raw[4];
    //             rchandle.newAddress[3] = rchandle.rx_msg.raw[5];
    //             rchandle.newAddress[4] = rchandle.rx_msg.raw[6];
    //             rchandle.newRfChannel = rchandle.rx_msg.raw[7];

    //             rt_memset(rchandle.tx_msg.raw, 0, sizeof(rchandle.tx_msg.raw));
    //             rchandle.tx_msg.v.head = WL_MSG_HEAD_PAIR_REQ;
    //             rchandle.tx_msg.v.index = ackCnt;
    //             rchandle.tx_msg.raw[2] = rchandle.newAddress[0];
    //             rchandle.tx_msg.raw[3] = rchandle.newAddress[1];
    //             rchandle.tx_msg.raw[4] = rchandle.newAddress[2];
    //             rchandle.tx_msg.raw[5] = rchandle.newAddress[3];
    //             rchandle.tx_msg.raw[6] = rchandle.newAddress[4];
    //             rchandle.tx_msg.raw[7] = rchandle.newRfChannel;

    //             rchandle.pairConfirmTime = 0;
    //             ackCnt = 0;
    //         }
    //         else if (rchandle.rx_msg.v.head == WL_MSG_HEAD_PAIR_COMFIR) // 对方确认
    //         {
    //             rchandle.tx_msg.v.head = WL_MSG_HEAD_PAIR_COMFIR;
    //             rchandle.tx_msg.v.index = ackCnt;
    //             LOG_D("WL_MSG_HEAD_PAIR_COMFIR:%d\n", rchandle.rx_msg.v.index);
    //             if (rchandle.rx_msg.v.index > 2)
    //             {
    //                 rchandle.pairConfirmTime = millis();
    //             }
    //         }
    //         rt_memset(rchandle.rx_msg.v.channel, 0, sizeof(rchandle.rx_msg.v.channel));
    //     } // end of pair
    //     else // 通常情况回复飞控状态
    //     {
    //         // FC_ERROR_SET(FC_STANDBY);
    //         rchandle.tx_msg.v.head = WL_MSG_HEAD_FC_STATUS;
    //         rchandle.tx_msg.v.index = ackCnt;
    //         // rchandle.tx_msg.raw[2] = (fc_error_flag >> 24) & 0xff;
    //         // rchandle.tx_msg.raw[3] = (fc_error_flag >> 16) & 0xff;
    //         // rchandle.tx_msg.raw[4] = (fc_error_flag >> 8) & 0xff;
    //         // rchandle.tx_msg.raw[5] = (fc_error_flag) & 0xff;
    //     }
    // } // end of crc ok
    // rchandle.tx_msg.v.crc = crc8_dvb_s2_update(CRC_MAGICBEE, rchandle.tx_msg.raw, RTX_BUFFER_SIZE - 1);
    // si24r1_send_data(s, rchandle.tx_msg.raw, 8, 0); // 8 bytes ack enough, or would lost signal
    // ackCnt++;

    // LOG_D("RC:%dHz \n",
    // rchandle.rx_rssi
    // );
}

/**
 * @brief 发送完成处理函数
 *
 * @param nrf24
 * @param pipe
 */
static void tx_done(struct si24r1 *s, int pipe)
{
    static int cnt = 0;
    // static char tbuf[32];

    cnt++;

    /*! Here just want to tell the user when the role is ROLE_PTX
    the pipe have no special meaning except indicating (send) FAILED or OK
        However, it will matter when the role is ROLE_PRX*/
    s->port.ops->write_reg(&s->port, SI24R1_REG_CONFIG, (uint8_t *)&(s->onchip_config.config), 1);
    if (s->user_config.role == SI24R1_ROLE_TX)
    {
        // if (pipe == 8)
        //     rt_kprintf("tx_done failed");
        // else
        //     rt_kprintf("tx_done ok");
    }
    else
    {
        // rt_kprintf("tx_done ok");
    }

    // rt_kprintf(" (pipe%d)\n", pipe);

    // rt_sprintf(tbuf, "My role is %s [%dth]\n", ROLE_TABLE[s->user_config.role], cnt);
    // si24r1_send_data(s, (uint8_t*)tbuf, rt_strlen(tbuf), pipe);
#ifdef PKG_NRF24L01_DEMO_ROLE_PTX
    rt_thread_mdelay(NRF24_DEMO_SEND_INTERVAL);
#endif
}

struct si24r1_callback si24r1_cb = {
    .rx_ind = rx_ind,
    .tx_done = tx_done,
};
/**
 * @brief 摇控器si24r1的配置，
 * 信号丢失时，参数变化时，需要调用一下来更新si24r1的寄存器配置
 *
 * @param rc
 */
void rc_si24r1_setting(struct rchandle *rc)
{

    // #if 0
    //     rt_memcpy(rc->address, "test3", 5);
    //     rc->rf_channel = 125;
    // #else
    //     rt_memcpy(rc->address, "test4", 5);
    //     rc->rf_channel = 130;
    // #endif
    rt_memset(&rc->si24r1.onchip_config, 0, sizeof(rc->si24r1.onchip_config));
    if (rc->mode == RC_MODE_PAIRING)
    {
        rt_memcpy(rc->si24r1.onchip_config.tx_addr.addr, rc->pair_addr, 5);
        rt_memcpy(rc->si24r1.onchip_config.rx_addr_p0.addr, rc->pair_addr, 5);
        rc->si24r1.onchip_config.rf_ch.RF_CH = rc->pair_channel;
    }
    else
    {
        rt_memcpy(rc->si24r1.onchip_config.tx_addr.addr, rc->address, 5);
        rt_memcpy(rc->si24r1.onchip_config.rx_addr_p0.addr, rc->address, 5);
        rc->si24r1.onchip_config.rf_ch.RF_CH = rc->rf_channel;
    }

    rc->si24r1.onchip_config.en_rxaddr.ERX_P0 = 1;
    rc->si24r1.onchip_config.rx_pw_p0.RX_PW_P0 = RTX_BUFFER_SIZE;
    rc->si24r1.onchip_config.setup_aw.AW = 0x03;
    rc->si24r1.onchip_config.feature.EN_DPL = 1;
    rc->si24r1.onchip_config.feature.EN_ACK_PAYd = 1;
    rc->si24r1.onchip_config.rf_setup.RF_DR_HIGH = 1;
    rc->si24r1.onchip_config.rf_setup.RF_PWR = 7;
    rc->si24r1.onchip_config.config.EN_CRC = 1;
    rc->si24r1.onchip_config.config.CRCO = 1;
    rc->si24r1.onchip_config.config.PWR_UP = 1;
    rc->si24r1.onchip_config.config.PRIM_RX = 1;
    rc->si24r1.onchip_config.dynpd.DPL_P0 = 1;

    rc->si24r1.onchip_config.status.MAX_RT = 1; // 清除中断
    rc->si24r1.onchip_config.status.RX_DR = 1;  // 清除中断
    rc->si24r1.onchip_config.status.TX_DS = 1;  // 清除中断
    rc->si24r1.onchip_config.en_aa.ENAA_P0 = 1;
    rc->si24r1.onchip_config.setup_retr.ARC = 10; // 10次
    rc->si24r1.onchip_config.setup_retr.ARD = 0;  // 250us

    rc->si24r1.user_config.role = SI24R1_ROLE_RX;

    si24r1_update_onchip_config(&rc->si24r1);
    // si24r1_check_all_reg(&rc->si24r1);
}

int hw_si24r1_init(struct si24r1 *dev)
{
    rt_err_t ret = RT_EOK;
    // 循环初始化si24r1，直到成功
    do
    {
        ret = hal_si24r1_port_init(&dev->port, 0, SI24R1_CE_PIN, SI24R1_IRQ_PIN, 0, 0);
        // ret = si24r1_init(si, SI24R1_SPI_DEV_NAME, SI24R1_CE_PIN, SI24R1_IRQ_PIN, &si24r1_cb);
        if (ret != RT_EOK)
        {
            LOG_E("Failed to init si24r1. Next try in 3 second.");
            rt_thread_mdelay(100);
        }
    } while (ret != RT_EOK);
    LOG_I("si24r1_thread running...");
    rchandle.si24r1.flags.using_irq = 1;

    rc_si24r1_setting(&rchandle);
    rt_thread_mdelay(1);
    si24r1_check_all_reg(&rchandle.si24r1);
    // rt_sem_release(rchandle.si24r1.sem);
    // while (1)
    // {
    //     si24r1_run(si);
    //     if (!si->flags.using_irq)
    //     {
    //     }
    //     rt_thread_mdelay(10);
    // }
    return 0;
}
void rc_into_pair_mode()
{
    if (rchandle.mode != RC_MODE_PAIRING)
    {
        rchandle.mode = RC_MODE_PAIRING;
        // FC_ERROR_SET(FC_RC_PAIRING);
        // FC_ERROR_RESET(FC_RC_LOST);
        rchandle.pair_sync_time = 0;
        rchandle.pairConfirmTime = 0;
        LOG_D("pair mode \n\r");
    }
}

void rc_into_normal_mode()
{
    if (rchandle.mode != RC_MODE_NORMAL)
    {
        // FC_ERROR_RESET(FC_RC_PAIRING);
        rchandle.mode = RC_MODE_NORMAL;
        rchandle.pair_sync_time = 0;
        rchandle.pairConfirmTime = 0;
        LOG_D("normal mode \n\r");
    }
}
void rc_keyActionHandle(uint8_t times)
{
    LOG_D("keyActionHandle times:%d %d %d\n", times, rchandle.mode, rchandle.pair_sync_time);
    if (times == 2)
    {
        if (rchandle.pair_sync_time > 3)
        {
            rc_into_normal_mode();
            // rc_si24r1_setting(rc);
        }
        else
        {
            rc_into_pair_mode();
        }
    }
}
static void rc_second_timer_cb(void *parameter)
{
    struct rc_handle *rc = (struct rc_handle *)parameter;
    static uint8_t lostCnt = 0;
    rc->rx_rssi = rc->rxCnt;
    rc->rxCnt = 0;
    // LOG_D("RC:%dHz %08X\n",
    //     rchandle.rx_rssi
    //     , fc_error_flag);
    if (rc->mode == RC_MODE_PAIRING)
    {
        rc->pair_sync_time++;
        if (rc->pair_sync_time >= rc->pair_wait_time)
        {
            // LOG_E("pair fail!\n");
            rc->pair_sync_time = 0;
            rc->pairConfirmTime = 0;
            rc_into_normal_mode();
            rt_event_send(rc->event, RC_EVENT_PAIRING_FAIL);
        }
    }
    if (rc->rx_rssi == 0)
    {
        if (lostCnt++ == 1)
        {
            lostCnt = 0;
            rt_event_send(rc->event, RC_EVENT_LOST);
        }
        if ((rc->mode == RC_MODE_PAIRING) && (rc->pairConfirmTime != 0) && millis() > (1000 + rc->pairConfirmTime))
        {
            // NOTE 对频模式，收到确认，然后没信号了，说明遥控器也收到确认了，然后跳到新频道上了。

            rt_event_send(rc->event, RC_EVENT_PAIRING_SUCCESS);
        }
    }
}
void rc_thread(void *param)
{
    struct rc_handle *rc = (struct rc_handle *)param;
    rc->event = rt_event_create("rc_event", RT_IPC_FLAG_FIFO);
    rc->second_timer = rt_timer_create("rc_1s", rc_second_timer_cb, rc, 1000, RT_TIMER_FLAG_PERIODIC);
    rt_timer_start(rc->second_timer);

    while (1)
    {
        rt_uint32_t event = 0;
        if (rt_event_recv(rc->event, RC_EVENT_ALL, RT_EVENT_FLAG_OR | RT_EVENT_FLAG_CLEAR, RT_WAITING_FOREVER, &event) == RT_EOK)
        {
            // LOG_D("rc_event: 0x%08X", event);
            switch (event)
            {
            case RC_EVENT_LOST:
                // LOG_D("RC_EVENT_LOST %d %d", rchandle.mode, rchandle.pairConfirmTime);
                rc_si24r1_setting(rc);
                // if (rc->mode == RC_MODE_PAIRING)
                // si24r1_check_all_reg(&rc->si24r1);
                // rt_sem_release(rc->si24r1.sem);
                break;
            case RC_EVENT_PAIRING:
                LOG_D("RC_EVENT_PAIRING");
                break;
            case RC_EVENT_PAIRING_SUCCESS:
            {

                rc->address[0] = rc->newAddress[0];
                rc->address[1] = rc->newAddress[1];
                rc->address[2] = rc->newAddress[2];
                rc->address[3] = rc->newAddress[3];
                rc->address[4] = rc->newAddress[4];
                rc->rf_channel = rc->newRfChannel;
                pair_data()->address[0] = rc->address[0];
                pair_data()->address[1] = rc->address[1];
                pair_data()->address[2] = rc->address[2];
                pair_data()->address[3] = rc->address[3];
                pair_data()->address[4] = rc->address[4];
                pair_data()->rf_channel = rc->rf_channel;
                pair_data()->flag = 0xdd;
                rc_into_normal_mode();
                zino_config_save_all();
                rc->pairConfirmTime = 0;
                rc->pair_sync_time = 0;
                LOG_D("RC_EVENT_PAIRING_SUCCESS");
                rc_si24r1_setting(rc);
                // si24r1_check_all_reg(&rc->si24r1);
            }
            break;
            case RC_EVENT_PAIRING_FAIL:
                // rc_si24r1_setting(rc);
                LOG_E("RC_EVENT_PAIRING_FAIL");
                break;
            default:
                break;
            }
        }
    }
}
int rc_task_init(void)
{
    struct rc_handle *rc = &rchandle;

    if (pair_data()->flag == 0xdd)
    {
        rchandle.address[0] = pair_data()->address[0];
        rchandle.address[1] = pair_data()->address[1];
        rchandle.address[2] = pair_data()->address[2];
        rchandle.address[3] = pair_data()->address[3];
        rchandle.address[4] = pair_data()->address[4];
        rchandle.rf_channel = pair_data()->rf_channel;
        LOG_D("Loading save data:\n");
        LOG_D("address[0] = %x\n", rchandle.address[0]);
        LOG_D("address[1] = %x\n", rchandle.address[1]);
        LOG_D("address[2] = %x\n", rchandle.address[2]);
        LOG_D("address[3] = %x\n", rchandle.address[3]);
        LOG_D("address[4] = %x\n", rchandle.address[4]);
        LOG_D("rf_channel = %x\n", rchandle.rf_channel);
    }
    hw_si24r1_init(&rc->si24r1);
    // set_hb_key_action_callback(keyActionHandle);
    // thread1 = rt_thread_create("2.4G wl", si24r1_thread, &rc->si24r1, 512, RT_THREAD_PRIORITY_MAX / 2, 20);
    // thread2 = rt_thread_create("rc task", rc_thread, rc, 512, RT_THREAD_PRIORITY_MAX / 2, 20);
    // if (thread1 != RT_NULL)
    //     rt_thread_startup(thread1);
    // else
    //     LOG_E("2.4G wl task creat fail!\n");
    // if (thread2 != RT_NULL)
    //     rt_thread_startup(thread2);
    // else
    //     LOG_E("rc task creat fail!\n");

    // LOG_I("rc_task_init");
    return RT_EOK;
}
#include "stdlib.h"
void rc_set(int arg_c, char **arg_v)
{
    rchandle.rf_channel = atoi(arg_v[1]);
    LOG_D("set rf channel:%d", rchandle.rf_channel);
    rc_si24r1_setting(&rchandle);
    si24r1_check_all_reg(&rchandle.si24r1);
}

uint16_t get_rc_channel(uint8_t rcchannel)
{
    if (rcchannel < 8)
        return rchandle.rx_msg.v.channel[rcchannel];
    return 0;
}

struct rc_handle *get_rc_channel_ram(void)
{
    return &rchandle;
}

// MSH_CMD_EXPORT(rc_set, set rc);
// ZINO_APP_EXPORT(rc_task_init);
